﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_MergeDynObjectList.cs
 * \brief	This file contains the MergeDynObjRobot function that  finds pairs the objects find in the current iteration with the global list 
         *          of object from this robot
 *  
 * This file contains the ObjectIdentification function that  finds pairs the objects find in the current iteration with the global list 
         *          of object from this robot
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                          2010-02-16 \n
 *      First implementation of the M44_MergeDynObjectList class.
 *
 * - Version 0.02:   Bjoern STEURER                             2010-04-22 \n
 *      Moved to M44_SharedClasses, namespace changed accordingly 
 *      
 * - Version 0.03: Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 *      
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SharedClasses
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SharedClasses
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_MergeDynObjectList
     * 
     *  \brief  This is the class of the M44_MergeDynObjectList function.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_MergeDynObjRobots
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_DynamicObjects MergeDynObjRobots(cl_DynamicObjects RobotObjectList,
                                                                    ArrayList RobArray)
         * 
         *  \brief This function finds pairs the objects find in the current iteration with the global list 
         *          of object from this robot
         *     
         *  \param[in] cl_DynamicObjects RobotObjectList  current list of object and properties
         *  \param[in] ArrayList RobArray
         *  
         *  \param[out] cl_DynamicObjects 
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_DynamicObjects MergeDynObjRobots(cl_DynamicObjects RobotObjectList,
                                                          ArrayList RobArray)
        {
               
            int n_rob;
            int n_list;
            double[,] rob_pos, list_pos, posible_pairs, selected_pairs;
            bool first = true;
            double distance = 0;
            int i_aux,i;
            double dist_min;
            double distance_tol = DynObj_Const.f_object_distance_tol;
            bool[] obj_rob_paired;
            bool[] obj_list_paired;
            double[] pos1 = new double[2];
            cl_DynamicObjects DynObjAux = new cl_DynamicObjects();
            int n_pairs_ok = 0;
            n_rob = RobArray.Count;
            obj_rob_paired = new bool[n_rob];
            n_list = RobotObjectList.List.Length;
            obj_list_paired = new bool[n_list];

            //Pair the objects and the robots
            if (n_rob == 0 && n_list > 0)
            {
                DynObjAux = RobotObjectList;
            }
            else if (n_rob >0 && n_list == 0)
            {
                DynObjAux = new cl_DynamicObjects((ushort)n_rob);
                //Create dynamic object list with the robots
                i = 0;
                foreach (cl_RobotPosition RobotPos in RobArray)
                {
                    DynObjAux.List[i].f_DetectionTime = (float)RobotPos.d_AbsTime;
                    DynObjAux.List[i].Position.d_x = (double)RobotPos.Position.f_x;
                    DynObjAux.List[i].Position.d_y = (double)RobotPos.Position.f_y;
                    DynObjAux.List[i].us_Identification = (ushort)(RobotPos.s_RobotID * 1000);
                    DynObjAux.List[i].Velocity.d_xVel = (double)RobotPos.Velocity.f_x;
                    DynObjAux.List[i].Velocity.d_yVel = (double)RobotPos.Velocity.f_y;
                    DynObjAux.List[i].PhysCharac.c_ObjectClass = 'r';
                    DynObjAux.List[i].PhysCharac.c_ObjectType = 'v'; 
                    i++;
                }
            }
            else if (n_rob > 0 && n_list > 0)
            {
                rob_pos = new double[n_rob, 2];
                list_pos = new double[n_list, 2];
                posible_pairs = new double[n_list * n_rob, 3];
                selected_pairs = new double[Math.Min(n_rob, n_list), 3];

                //Calculate the distance between the points
                i = 0;
                foreach (cl_RobotPosition RobotPos in RobArray)
                {
                    rob_pos[i, 0] = RobotPos.Position.f_x;
                    rob_pos[i, 1] = RobotPos.Position.f_y;
                    for (int j = 0; j < n_list; j++)
                    {
                        if (first)
                        {
                            list_pos[j, 0] = RobotObjectList.List[j].Position.d_x;
                            list_pos[j, 1] = RobotObjectList.List[j].Position.d_y;
                        }
                        //Calculate the distance
                        distance = Math.Sqrt((Math.Pow((list_pos[j, 0] - rob_pos[i, 0]), 2) +
                                              Math.Pow((list_pos[j, 1] - rob_pos[i, 1]), 2)));
                        //Save the pair
                        posible_pairs[j + i * n_rob, 0] = i;
                        posible_pairs[j + i * n_rob, 1] = j;
                        posible_pairs[j + i * n_rob, 2] = distance;
                    }
                    first = false;
                    i++;
                }
                //Order the pairs
                dist_min = 1000000;
                i_aux = 0;
                while (i_aux < Math.Min(n_rob, n_list))
                {
                    for (int i2 = 0; i2 < (n_rob * n_list); i2++)
                    {
                        if (posible_pairs[i2, 2] < dist_min && posible_pairs[i2, 2] > 0)
                        {
                            selected_pairs[i_aux, 0] = posible_pairs[i2, 0];
                            selected_pairs[i_aux, 1] = posible_pairs[i2, 1];
                            selected_pairs[i_aux, 2] = posible_pairs[i2, 2];
                            dist_min = posible_pairs[i2, 2];
                        }
                    }
                    for (int i2 = 0; i2 < (n_rob * n_list); i2++)
                    {
                        if (posible_pairs[i2, 0] == selected_pairs[i_aux, 0])
                            posible_pairs[i2, 2] = -1;
                        if (posible_pairs[i2, 1] == selected_pairs[i_aux, 1])
                            posible_pairs[i2, 2] = -1;
                    }
                    dist_min = 1000000;
                    i_aux++;

                }
                //Check each pair to see if distance its acceptable and to merge the data
                for (int i2 = 0; i2 < Math.Min(n_rob, n_list); i2++)
                {
                    if (selected_pairs[i2, 2] < distance_tol)
                    {
                        n_pairs_ok++;
                    }
                }
                DynObjAux = new cl_DynamicObjects((ushort)(n_list + n_rob - n_pairs_ok));
                i_aux = 0;
                cl_RobotPosition RobotPos2;
                while (i_aux < n_list + n_rob - n_pairs_ok)
                {
                    for (int i2 = 0; i2 < Math.Min(n_rob, n_list); i2++)
                    {
                        if (selected_pairs[i2, 2] < distance_tol)
                        {
                            obj_rob_paired[(int)selected_pairs[i2, 0]] = true;
                            obj_list_paired[(int)selected_pairs[i2, 1]] = true;
                            RobotPos2 = (cl_RobotPosition)RobArray[(int)selected_pairs[i2, 0]];
                            DynObjAux.List[i_aux].f_DetectionTime = (float)RobotPos2.d_AbsTime;
                            

                            DynObjAux.List[i_aux].PhysCharac.c_ObjectClass = 'r';
                            DynObjAux.List[i_aux].PhysCharac.c_ObjectType = 'v';

                            DynObjAux.List[i_aux].Position.d_x = (double)RobotPos2.Position.f_x;
                            DynObjAux.List[i_aux].Position.d_y = (double)RobotPos2.Position.f_y;
                            DynObjAux.List[i_aux].us_Identification = (ushort)(RobotPos2.s_RobotID * 1000);
                            DynObjAux.List[i_aux].Velocity.d_xVel = (double)RobotPos2.Velocity.f_x;
                            DynObjAux.List[i_aux].Velocity.d_yVel = (double)RobotPos2.Velocity.f_y;
                            i_aux++;

                        }
                    }
                    for (int i2 = 0; i2 < n_list; i2++)
                    {
                        if (!obj_list_paired[i2])
                        {
                            DynObjAux.List[i_aux] = RobotObjectList.List[i2];
                            i_aux++;
                        }
                    }
                    for (int i2 = 0; i2 < n_rob; i2++)
                    {
                        if (!obj_rob_paired[i2])
                        {
                            RobotPos2 = (cl_RobotPosition)RobArray[i2];
                            DynObjAux.List[i_aux].f_DetectionTime = (float)RobotPos2.d_AbsTime;
                            DynObjAux.List[i_aux].Position.d_x = (double)RobotPos2.Position.f_x;
                            DynObjAux.List[i_aux].Position.d_y = (double)RobotPos2.Position.f_y;
                            DynObjAux.List[i_aux].us_Identification = (ushort)(RobotPos2.s_RobotID * 1000);
                            DynObjAux.List[i_aux].Velocity.d_xVel = (double)RobotPos2.Velocity.f_x;
                            DynObjAux.List[i_aux].Velocity.d_yVel = (double)RobotPos2.Velocity.f_y;
                            DynObjAux.List[i_aux].PhysCharac.c_ObjectClass = 'r';
                            DynObjAux.List[i_aux].PhysCharac.c_ObjectType = 'v';
                            i_aux++;
                        }
                    }
                }
            }
            return DynObjAux;
        }
    }
}
